/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef IO_H_
#define IO_H_

/*****************************************************************************
** Includes
*****************************************************************************/
//hpp
#include <memory>
#include <string>

#include <poll.h> // should get cmake to explicitly check for poll.h?
#include <sys/poll.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <unistd.h>
#include <netdb.h>       // getnameinfo in network.cpp
#include <netinet/in.h>  // sockaddr_in in network.cpp
#include <netinet/tcp.h> // TCP_NODELAY in transport/transport_tcp.cpp                                                                     





//cpp
//#include <ros/assert.h> // don't need if we dont call the pipe functions.
//#include <ros/file_log.h>
#include <errno.h> // for EFAULT and co.
#include <iostream>
#include <sstream>
#include <cstring> // strerror
#include <fcntl.h> // for non-blocking configuration

#include <sys/epoll.h>


//util
#include "../util/Trace.hpp"

/*****************************************************************************
** Macros
*****************************************************************************/
//hpp
    #define ROS_SOCKETS_SHUT_RDWR SHUT_RDWR /* Used by ::shutdown() */
    #define ROS_INVALID_SOCKET ((int) -1)
    #define ROS_SOCKETS_ASYNCHRONOUS_CONNECT_RETURN EINPROGRESS



//cpp
#define UNUSED(expr) do { (void)(expr); } while (0)

/*****************************************************************************
** HPP
*****************************************************************************/
typedef int socket_fd_t;
typedef int signal_fd_t;
typedef struct pollfd socket_pollfd;

typedef std::shared_ptr<std::vector<socket_pollfd> > pollfd_vector_ptr;
int last_socket_error();
const char* socket_error_string(int err);
const char* last_socket_error_string();
bool last_socket_error_is_would_block();
pollfd_vector_ptr poll_sockets(int epfd, socket_pollfd *fds, nfds_t nfds, int timeout);
int is_async_connected(socket_fd_t &socket, int &err);
int set_non_blocking(socket_fd_t &socket);
int close_socket(socket_fd_t &socket);
int create_signal_pair(signal_fd_t signal_pair[2]);

int create_socket_watcher();
void close_socket_watcher(int fd);
void add_socket_to_watcher(int epfd, int fd);
void del_socket_from_watcher(int epfd, int fd);
void set_events_on_socket(int epfd, int fd, int events);
/**
 * Closes the signal pair - on windows we're using sockets (because windows
 * select() function cant handle pipes). On linux, we're just using the pipes.
 * @param signal_pair : the signal pair type.
 */
inline void close_signal_pair(signal_fd_t signal_pair[2]) {
// use a socket pair on mingw or pipe pair on linux, either way, close works
    ::close(signal_pair[0]);
    ::close(signal_pair[1]);
}

/**
 * Write to a signal_fd_t device. On windows we're using sockets (because windows
 * select() function cant handle pipes) so we have to use socket functions.
 * On linux, we're just using the pipes.
 */
    inline ssize_t write_signal(const signal_fd_t &signal, const void *buffer, const size_t &nbyte) {
        return write(signal, buffer, nbyte);
    }


/**
 * Read from a signal_fd_t device. On windows we're using sockets (because windows
 * select() function cant handle pipes) so we have to use socket functions.
 * On linux, we're just using the pipes.
 */
    inline ssize_t read_signal(const signal_fd_t &signal, void *buffer, const size_t &nbyte) {
        return read(signal, buffer, nbyte);
    }



/*****************************************************************************
** CPP
*****************************************************************************/


int last_socket_error() {
		return errno;
}

const char* socket_error_string(int err) {
    return strerror(err);
}

const char* last_socket_error_string() {
        return socket_error_string(errno);
}

bool last_socket_error_is_would_block() {
	if ( ( errno == EAGAIN  ) || ( errno == EWOULDBLOCK ) ) { // posix permits either
		return true;
	} else {
		return false;
	}
}

int create_socket_watcher() {
  int epfd = -1;
  epfd = ::epoll_create1(0);
  if (epfd < 0)
  {
    TRACE("[ERROR] Unable to create epoll watcher: ", strerror(errno));
  }
  return epfd;
}

void close_socket_watcher(int fd) {
  if (fd >= 0)
    ::close(fd);
}

void add_socket_to_watcher(int epfd, int fd) {
  struct epoll_event ev;
  bzero(&ev, sizeof(ev));

  ev.events = 0;
  ev.data.fd = fd;

  if (::epoll_ctl(epfd, EPOLL_CTL_ADD, fd, &ev))
  {
    TRACE("[ERROR] Unable to add FD to epoll: ", strerror(errno));
  }
}

void del_socket_from_watcher(int epfd, int fd) {
  if (::epoll_ctl(epfd, EPOLL_CTL_DEL, fd, NULL))
  {
    TRACE("[ERROR] Unable to remove FD to epoll: ", strerror(errno));
  }
}

void set_events_on_socket(int epfd, int fd, int events) {
  struct epoll_event ev;
  bzero(&ev, sizeof(ev));

  ev.events = events;
  ev.data.fd = fd;
  if (::epoll_ctl(epfd, EPOLL_CTL_MOD, fd, &ev))
  {
    TRACE("[ERROR] Unable to modify FD epoll: ", strerror(errno));
  }
}



/*****************************************************************************
** Service Robotics/Libssh Functions
*****************************************************************************/
/**
 * @brief A cross platform polling function for sockets.
 *
 * Windows doesn't have a polling function until Vista (WSAPoll) and even then
 * its implementation is not supposed to be great. This works for a broader
 * range of platforms and will suffice.
 * @param epfd - the socket watcher to poll on.
 * @param fds - the socket set (descriptor's and events) to poll for.
 * @param nfds - the number of descriptors to poll for.
 * @param timeout - timeout in milliseconds.
 * @return pollfd_vector_ptr : NULL on error, empty on timeout, a list of structures with received events.
 */
pollfd_vector_ptr poll_sockets(int epfd, socket_pollfd *fds, nfds_t nfds, int timeout) {
	UNUSED(nfds);
	UNUSED(fds);
	struct epoll_event ev[nfds];
	pollfd_vector_ptr ofds;

	int fd_cnt = ::epoll_wait(epfd, ev, nfds, timeout);

	if (fd_cnt < 0)
	{
	// EINTR means that we got interrupted by a signal, and is not an error
		if(errno != EINTR) {
			TRACE("[ERROR] Error in epoll_wait! ", strerror(errno));
			ofds.reset();
		}
	}
	else
	{
		ofds.reset(new std::vector<socket_pollfd>);
		for (int i = 0; i < fd_cnt; i++)
		{
			socket_pollfd pfd;
			pfd.fd = ev[i].data.fd;
			pfd.revents = ev[i].events;
			ofds->push_back(pfd);
		}
	}
	return ofds;
}
/*****************************************************************************
** Socket Utilities
*****************************************************************************/
/**
 * Checks if the async socket connection is success, failure or connecting.
 * @param err - WSAGetLastError()/errno on failure.
 * @return int : 1 on connected, 0 on connecting, -1 on failure.
 */
int is_async_connected(socket_fd_t &socket, int &err) {
    int nfds = 1;
    int epfd = create_socket_watcher();
    add_socket_to_watcher(epfd, socket);
    set_events_on_socket(epfd, socket, EPOLLIN | EPOLLOUT);
    struct epoll_event ev[nfds];
    int fd_cnt = ::epoll_wait(epfd, ev, nfds, 0);
    close_socket_watcher(epfd);
    if (fd_cnt < 0) {
      if (errno != EINTR) {
        TRACE("[ERROR] Error in epoll_wait! ", strerror(errno));
        err = errno;
        return -1;
      }
    } else if (fd_cnt == 0) {
      err = 0;
      return 0;
    } else if (ev[0].events & EPOLLERR || ev[0].events & EPOLLHUP) {
      // attempt to retrieve the error on the file descriptor epoll is waiting on
      err = 0;
      socklen_t errlen = sizeof(err);
      getsockopt(socket, SOL_SOCKET, SO_ERROR, (void *)&err, &errlen);
      return -1;
    }
    err = 0;
    return 1;
}

/**
 * Sets the socket as non blocking.
 * @return int : 0 on success, WSAGetLastError()/errno on failure.
 */
int set_non_blocking(socket_fd_t &socket) {
    if(fcntl(socket, F_SETFL, O_NONBLOCK) == -1)
    {
      return errno;
    }
    return 0;
}

/**
 * @brief Close the socket.
 *
 * @return int : 0 on success, -1 on failure.
 */
int close_socket(socket_fd_t &socket) {
	if (::close(socket) < 0) {
		return -1;
	} else {
		return 0;
	}
}

/*****************************************************************************
** Signal Pair
*****************************************************************************/
/**
 * This code is primarily from the msdn socket tutorials.
 * @param signal_pair : a pair of sockets linked to each other over localhost.
 * @return 0 on success, -1 on failure.
 */
int create_signal_pair(signal_fd_t signal_pair[2]) {
	// initialize
	signal_pair[0] = -1;
	signal_pair[1] = -1;

	if(pipe(signal_pair) != 0) {
		TRACE( "[FATAL] pipe() failed");
		return -1;
	}
	if(fcntl(signal_pair[0], F_SETFL, O_NONBLOCK) == -1) {
		TRACE( "[FATAL] fcntl() failed");
		return -1;
	}
	if(fcntl(signal_pair[1], F_SETFL, O_NONBLOCK) == -1) {
		TRACE( "[FATAL] fcntl() failed");
		return -1;
	}
	return 0;
}



#endif  // #ifndef IO_H_
